import threading
import rospy
from sensor_msgs.msg import Image
import cv2
import ros_numpy

class Camera():

    def __init__(self):
        self.depthimg = None
        self.img = None
        self.imageros = None

        self.depthFlag = False
        self.imgFlag = False
        
        #rospy.init_node('depth_image_subscriber', anonymous=True)
        camgetimage = threading.Thread(target=self._cam)
        camgetimage.start()

        while self.imgFlag == False or self.depthFlag == False:
            cv2.waitKey(1)
        print("Camara Node Inited.")

    def _cam(self):
        rospy.Subscriber('/camera/aligned_depth_to_color/image_raw', Image, self.depth_image_callback)
        rospy.Subscriber('/camera/color/image_raw',Image,self.image_callback)
        rospy.Subscriber('/camera/color/image_raw',Image,self.image_ros_callback)
        rospy.spin()

    def image_ros_callback(self,msg):
        
        self.imageros = msg

    def depth_image_callback(self,msg):
        image = ros_numpy.numpify(msg)
        self.depthimg = image
        self.depthFlag = True

    def image_callback(self,msg):
        image = ros_numpy.numpify(msg)
        image = cv2.cvtColor(image,cv2.COLOR_BGR2RGB)
        self.img = image
        self.imgFlag = True
        
    def getimage(self):
        return self.img

    def getdepthimage(self):
        return self.depthimg
        
    def getdistance(self,point):
        x,y = point
        return self.depthimg[y,x]

    def getimageros(self):
        return self.imageros

if __name__ == "__main__":
    rospy.init_node('depth_image_subscriber', anonymous=True)
    cam = Camera()
    while 1:
        img = cam.getimage()
        depth_img = cam.getdepthimage()
        cv2.imshow("",img)
        cv2.imshow("1",depth_img)
        cv2.waitKey(1)
    
